#include "std_msgs/msg/float32_multi_array.hpp"
#include "ubt_state/ubt_state_util.h"
#include "ubt_state/ubt_state_api.h"
#include <fstream>
#include <iostream>
#include "ubt_state/node.h"

namespace ubt_state
{

//字符串分割函数
std::vector<std::string> split(std::string str, std::string pattern)
{
    std::string::size_type pos;
    std::vector<std::string> result;
    str += pattern;//扩展字符串以方便操作
    int size = str.size();
    for (int i = 0; i < size; i++)
    {
        pos = str.find(pattern, i);
        if (pos < size)
        {
            std::string s = str.substr(i, pos - i);
            result.push_back(s);
            i = pos + pattern.size() - 1;
        }
    }
    return result;
}

//去掉首尾空格
void trim(std::string &s)
{

    if( !s.empty() )
    {
        s.erase(0,s.find_first_not_of(" "));
        s.erase(s.find_last_not_of(" ") + 1);
    }

}

std::shared_ptr<spdlog::logger> ulogger;
//spdlog日志管理器
void initialSpdloger()
{
    try
    {
        string home_path = getenv("HOME");
        node->declare_parameter("log_path",home_path+"/.ros/log/ubt_and_middle.log");
        std::string path = home_path + node->get_parameter("log_path").as_string();
        static auto daily_sink = std::make_shared<spdlog::sinks::daily_file_sink_mt>(path, 23, 59);
        ulogger  = std::make_shared<spdlog::logger>("ubt_state",  daily_sink); 

        spdlog::register_logger(ulogger);
       
        spdlog::flush_every(std::chrono::seconds(5));
        //是否开启日志
        std::ifstream ifs;
        std::string target = "log.ubt.state=";
        
        ifs.open("/opt/ros/noetic/share/ros/config/rosconsole.config", std::ios::in);
        if (ifs.is_open())
        {         
            std::string buf;
            
            while (getline(ifs,buf))
            {
                if(buf.find(target) != std::string::npos)
                {
                    target =  split(buf,target)[1];
                    trim(target);
                }
            }
        }
        ifs.close();
        
        if(target == "ON")
        {
            spdlog::set_level(spdlog::level::info);
        }else
        {
            spdlog::set_level(spdlog::level::off);
        }
   }
   catch (const spdlog::spdlog_ex& ex)
   {
       std::cout << "Log initialization failed: " << ex.what() << std::endl;
   }
}


struct UtilClsData
{
    ObstacleParams obsParams;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr _pub_obstacle_dis;
};
static UtilClsData g_clsData;
geometry_msgs::msg::Pose2D PoseZero;

int UbtStateUtilInit()
{
    PoseZero.x = PoseZero.y = PoseZero.theta = 0.0;
    UtilClsData *pClsData = &g_clsData;
    pClsData->_pub_obstacle_dis = node->create_publisher<std_msgs::msg::Float32MultiArray>("/obstacle_dis", 1);
    return true;
}

void UbtStateUtilCleanup()
{
    UtilClsData *pClsData = &g_clsData;
    // pClsData->_pub_obstacle_dis->Shutdown();
}

int UbtStateUtilSetObstacleParams(ObstacleParams    obsParams)
{

    UtilClsData *pClsData = &g_clsData;
    pClsData->obsParams = obsParams;

    std_msgs::msg::Float32MultiArray msg;
    msg.data.push_back(obsParams.stopDis);
    msg.data.push_back(obsParams.slowDis);
    msg.data.push_back(obsParams.warnDis);
    msg.data.push_back(obsParams.sideDis);
    msg.data.push_back(obsParams.sideDis);
    g_clsData._pub_obstacle_dis->publish(msg);
    RCLCPP_INFO(node->get_logger(),"==================== exec ====================");
    return 0;
}

void UbtStateUtilGetObstacleParam(ObstacleParams& obsParams)
{
    obsParams = g_clsData.obsParams;
}

std::string UbtStateGetTaskResultDesc(int stateReason)
{

    std::string taskEvent;
    if (stateReason < ubt_task_result_com_max)
    {
        switch (stateReason)
        {
            case ubt_task_result_sys_antc:
                taskEvent = UBT_TASK_EVENT_SYS_ANTC_STR;
                break;
            case ubt_task_result_usr_stopbtn:
                taskEvent = UBT_TASK_EVENT_USR_STOPBTN_STR;
                break;
            case ubt_task_result_sys_nrfault:
                taskEvent = UBT_TASK_EVENT_SYS_NRFAULT_STR;
                break;
            case ubt_task_result_sys_poweroff:
                taskEvent = UBT_TASK_EVENT_SYS_POWEROFF_STR;
                break;
            case ubt_task_result_sys_posefail:
                taskEvent = UBT_TASK_EVENT_SYS_POSEFAIL_STR;
                break;
            case ubt_task_result_sys_mapupdate:
                taskEvent = UBT_TASK_EVENT_SYS_MAPUPDATE_STR;
                break;
            case ubt_task_result_sys_charge:
                taskEvent = UBT_TASK_EVENT_SYS_CHARGE_STR;
                break;
            case ubt_task_result_usr_cancel:
                taskEvent = UBT_TASK_EVENT_USR_CANCEL_STR;
                break;
            case ubt_task_result_usr_dispatchcancel:
                taskEvent = UBT_TASK_EVENT_USR_DISPATCHCANCEL_STR;
                break;
            case ubt_task_result_usr_brakebtn:
                taskEvent = UBT_TASK_EVENT_USR_BRAKEBTN_STR;
                break;
            case ubt_task_result_navigation_cancel:
                taskEvent = UBT_TASK_EVENT_NAVIGATION_CANCEL;
                break;
            default:
                ;
        }
    }
    else if ((stateReason >= ubt_task_result_nav_base) && (stateReason <= ubt_task_result_nav_max))
    {
        switch (stateReason)
        {
            case ubt_task_result_nav_aborte3m:
                taskEvent = UBT_TASK_EVENT_NAV_ABORTE3M_STR;
                break;
            case ubt_task_result_nav_posejump:
                taskEvent = UBT_TASK_EVENT_NAV_POSEJUMP_STR;
                break;
            case ubt_task_result_nav_trackfail:
                taskEvent = UBT_TASK_EVENT_NAV_TRACKFAIL_STR;
                break;
            default:
                taskEvent = UBT_TASK_EVENT_NAV_ABORT_STR;
        }
    }
    else if ((stateReason >= ubt_task_result_action_base) && (stateReason <= ubt_task_result_action_max))
    {
    }
    else if ((stateReason >= ubt_task_result_adjust_base) && (stateReason <= ubt_task_result_adjust_max))
    {
        taskEvent = UBT_TASK_EVENT_ADJUST_ABORT_STR;
    }
    else if ((stateReason >= ubt_task_result_charge_base) && (stateReason <= ubt_task_result_charge_max))
    {
        taskEvent = UBT_TASK_EVENT_CHARGE_ABORT_STR;
    }
    else
    {
    }
    RCLCPP_INFO(node->get_logger(),"==================== exec ====================");
    return taskEvent;
}

}



